﻿#include "lane_detection.h"
#include <opencv2/core/core.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include "calculate_tools.h"
//#include "dbscan_cluster.h"
#include "debug_show.h"
#include "stack"

//#define DEBUG_TEST
//#define DEBUG_TEST_LSM

namespace rmd {
LaneDetection::LaneDetection() {
  data_frame_ = NULL;
  lane_feature_extraction_ = new LaneFeatureExtraction();
  lane_modefit_ = new LaneModelFitting();

  ClassInitalization();
}

LaneDetection::~LaneDetection() {}

bool SortBypCenterBottom(const LaneLine& v1, const LaneLine& v2) {
  return v1.pCenterBottom < v2.pCenterBottom;
}

void LaneDetection::ClassInitalization() {
  current_show_lines.bflag = false;
  current_show_lines.left_top = kUnknowValue;
  current_show_lines.right_top = kUnknowValue;
  current_show_lines.left_location = kUnknowValue;
  current_show_lines.right_location = kUnknowValue;
  current_show_lines.left_bottom = kUnknowValue;
  current_show_lines.right_bottom = kUnknowValue;
  current_show_lines.left_index = -1;
  current_show_lines.right_index = -1;
  current_show_lines.left_rho = -1000;
  current_show_lines.right_rho = -1000;

  const_roi_continue = 0;
  single_line_continue = 0;
  save_good_vps.clear();
}

void LaneDetection::SetDataFrameToClass(DataFrame* data_frame,
                                        std::vector<cv::Rect>& vecicle_info) {
  data_frame_ = data_frame;
  this->process_image_ = data_frame->process_image;
  this->vehicle_info_ = vecicle_info;

  end_line_ = data_frame->end_line;
  middle_start_line_ = data_frame->middle_start_line;
  far_line_ = data_frame->far_line;
  start_line_ = data_frame->start_line;
  refine_lane_width_ = data_frame->refine_lane_width;
  m_user_setVP = data_frame->user_vanishing_point;
  off_x_length = data_frame->x_Length;
  off_y_length = data_frame->y_Length;

  data_frame_ = data_frame;
}

std::vector<LaneLine> LaneDetection::Update(
    DataFrame* data_frame, std::vector<cv::Rect>& vecicle_info) {
  std::vector<LaneLine> lane_lines;
  SetDataFrameToClass(data_frame, vecicle_info);

  cv::Mat pm_black(kImageHeight, kImageWidth, CV_8UC1, cv::Scalar(0, 0, 0));
  lane_feature_extraction_->Update(data_frame, pm_black);
  //  cv::imshow("pm_black", pm_black);

  // remove vehicle points
  if (vehicle_info_.size() > 0) {
    RemoveVehicleRectPoint(vehicle_info_, pm_black);
  }

  if (data_frame->auto_calibration_flag) {
    lane_modefit_->UpdateAuto(data_frame, pm_black);

    if (data_frame->auto_frame_index >=
        data_frame->Stable_Num * data_frame->Auto_Frame_Gap) {
      if (data_frame->auto_ld_vps.size() >=
          std::max(data_frame->Stable_Num * data_frame->Auto_Frame_Gap, 3)) {
        Point2f retVp = GetVanishingPointByLSM(data_frame->auto_ld_vps);
        if (data_frame->auto_select_ld_vps.size() > 1 &&
            abs(retVp.x - m_user_setVP.x) < off_x_length &&
            abs(retVp.y - m_user_setVP.y) < off_y_length) {
          if (data_frame->UpdateInit(retVp.x, retVp.y, false)) {
            data_frame->auto_calibration_flag = false;
          }
          data_frame->auto_ld_vps.clear();
          data_frame->auto_select_ld_vps.clear();
        }
      }
      //      else {
      //        data_frame->auto_ld_vps.clear();
      //      }

      if (data_frame->auto_frame_index > data_frame_->Stable_Num *
                                             data_frame_->Auto_Frame_Gap *
                                             data_frame_->Update_Gap) {
        data_frame->UpdateInit(m_user_setVP.x, m_user_setVP.y, false);
        data_frame->auto_calibration_flag = true;
        data_frame->auto_frame_index = 0;
        data_frame->auto_ld_vps.clear();
      }
    }
  } else {
    lane_lines = lane_modefit_->Update(data_frame, pm_black);

    long tIndex = lane_modefit_->GetIindex();

    data_frame->lIndex = tIndex;
    // UpdateParameter
    UpdateDataFrameParameter(data_frame, lane_lines);
    lane_lines = GetSelectLaneLines();
    //    TestRefineResult(lane_lines);
  }

  return lane_lines;
}

std::vector<LaneLine> LaneDetection::GetSelectLaneLines() {
  return select_lane_lines;
}

// Local Search Method
Point2f LaneDetection::GetVanishingPointByLSM(
    std::vector<INITLDVP>& condiate_vps) {
  Point2f retPointReuslt(-1, -1);
  int nVpSize = condiate_vps.size();

  if (nVpSize < 1) return retPointReuslt;

#ifdef DEBUG_TEST_LSM
  Mat showImage;
  data_frame_->process_image.copyTo(showImage);
  for (int i = 0; i < condiate_vps.size(); i++) {
    circle(showImage, Point2f(condiate_vps[i].x, condiate_vps[i].y), 1,
           Scalar(100, 0, 0), 2);
  }
#endif

  int maxNearTDis = std::min(off_x_length / 2, off_y_length / 2);
  int minClusterNum = 3;
  int scuClusterIndex = -1;
  std::vector<AutoCluster> corePoint;

  float avg_value_x = 0;
  float avg_value_y = 0;
  for (int i = 0; i < nVpSize; i++) {
    avg_value_x += condiate_vps[i].x;
    avg_value_y += condiate_vps[i].y;
    AutoCluster t_auto(i, condiate_vps[i].x, condiate_vps[i].y, i + 1);
    corePoint.push_back(t_auto);
  }
  avg_value_x = avg_value_x / (1.0 * nVpSize);
  avg_value_y = avg_value_y / (1.0 * nVpSize);

#ifdef DEBUG_TEST_LSM
  cv::circle(showImage, cv::Point2f(avg_value_x, avg_value_y), 1,
             Scalar(0, 255, 255), 3);
#endif

  int core_size = corePoint.size();

  auto square_distance = [&](cv::Point pt0, cv::Point pt1) -> float {
    return sqrt((pt0.x - pt1.x) * (pt0.x - pt1.x) +
                (pt0.y - pt1.y) * (pt0.y - pt1.y));
  };

  for (int ti = 0; ti < core_size; ti++) {
    for (int tj = ti + 1; tj < core_size; tj++) {
      float fdis = square_distance(corePoint[ti].point, corePoint[tj].point);
      if (fdis < maxNearTDis) {
        corePoint[ti].sum_value += fdis;
        corePoint[ti].pts++;
        corePoint[ti].corepts.push_back(tj);
        corePoint[tj].sum_value += fdis;
        corePoint[tj].pts++;
        corePoint[tj].corepts.push_back(ti);
      }
    }
  }

  // select top max pts cluster
  int min_cluster_distance = INT_MAX;
  int good_cluster_index = -1;
  int max_cluster_pts = -1;
  for (int i = 0; i < core_size; i++) {
    if (corePoint[i].pts >= minClusterNum &&
        corePoint[i].pts >= max_cluster_pts) {
      max_cluster_pts = corePoint[i].pts;
      corePoint[i].avg_value =
          corePoint[i].sum_value / (1.0 * corePoint[i].pts + 1e-6);
    }
  }

  for (int i = 0; i < core_size; i++) {
    if (corePoint[i].pts == max_cluster_pts &&
        corePoint[i].avg_value < min_cluster_distance) {
      min_cluster_distance = corePoint[i].avg_value;
      good_cluster_index = i;
    }
  }

  if (good_cluster_index >= 0 && good_cluster_index < core_size) {
    float t_size = t_size = corePoint[good_cluster_index].corepts.size();
    float t_point_x = 0;
    float t_point_y = 0;
    for (int t = 0; t < t_size; t++) {
      int t_index = corePoint[good_cluster_index].corepts[t];
      if (t_index >= 0 && t_index < core_size) {
        corePoint[t_index].visited = scuClusterIndex;
        t_point_x += corePoint[t_index].point.x;
        t_point_y += corePoint[t_index].point.y;
      }
    }

    if (t_size >= minClusterNum) {
      Point2f tCenterPoint2f =
          Point2f(t_point_x / (1.0 * t_size), t_point_y / (1.0 * t_size));
      if (abs(tCenterPoint2f.x - avg_value_x) < maxNearTDis &&
          abs(tCenterPoint2f.y - avg_value_y) < maxNearTDis)
        retPointReuslt = tCenterPoint2f;
    }
  }

  //  DbscanCluster::GetInstance()->DbscanMain(condiate_vps, maxNearTDis / 2,
  //                                           minClusterNum);

  if (retPointReuslt.x != -1 && retPointReuslt.y != -1) {
    data_frame_->auto_select_ld_vps.clear();
    for (int j = 0; j < nVpSize; j++) {
      if (corePoint[j].visited == scuClusterIndex) {
        data_frame_->auto_select_ld_vps.push_back(condiate_vps[j]);
      }
#ifdef DEBUG_TEST_LSM
      //      std::cout << "pCluster:" << corePoint[j].point << std::endl;
      cv::circle(showImage, cv::Point(condiate_vps[j].x, condiate_vps[j].y), 1,
                 cv::Scalar(255, 0, 255), 1);
#endif
    }
  }

#ifdef DEBUG_TEST_LSM
  cv::circle(showImage, retPointReuslt, 1, cv::Scalar(0, 255, 0), 2);
  cv::imshow("VanishingPointByLSM", showImage);
// waitKey(0);
#endif

  return retPointReuslt;
}

void LaneDetection::RemoveVehicleRectPoint(
    std::vector<cv::Rect>& _inVehicleInfo, cv::Mat& PMBlackOut) {
  uchar* p = PMBlackOut.data;
  int nRows = PMBlackOut.rows;
  int nCols = PMBlackOut.cols;

  int tCount = nRows * nCols;

  for (int i = 0; i < _inVehicleInfo.size(); i++) {
    int nStartX = _inVehicleInfo[i].x;
    int nStartY = _inVehicleInfo[i].y;
    int nWidth = _inVehicleInfo[i].width;
    int nHeight = _inVehicleInfo[i].height;

    for (int k = std::max(0, nStartY); k < nStartY + nHeight && k < nRows;
         k++) {
      for (int j = std::max(0, nStartX); j < nStartX + nWidth && j < nCols;
           j++) {
        if (k * nCols + j < tCount && p[k * nCols + j] != 0) {
          p[k * nCols + j] = 0;
        }
      }
    }
  }
}

void LaneDetection::UpdateDataFrameParameter(
    DataFrame* data_frame, std::vector<LaneLine>& lane_lines) {
  data_frame->pri_refine_results = lane_lines;
  std::vector<LaneLine> temp_select_lines;
  // remove low quality
  const int lane_lines_size = lane_lines.size();
  for (int i = 0; i < lane_lines_size; ++i) {
    if (lane_lines[i].nQuality >= kRefineQuality) {
      // filter some lines
      LaneLine tmp_fitspline = lane_lines[i];
      //      if (ExcludeOutLineByExtendPoint(tmp_fitspline))
      temp_select_lines.push_back(tmp_fitspline);
    }
  }

  //  std::cout << "quality_after:" << temp_select_lines.size() << std::endl;

  temp_select_lines = ExcludeOutLines(temp_select_lines);

  // fixed vp
  //  select_lane_lines = ExcludeOutLineByPoint(select_lane_lines);

  // update data_frame_->virtual_center_lines
  UpdateVisualCenterPoint(temp_select_lines);

  int line_size = temp_select_lines.size();
  if (line_size >= 2) {
    cv::Point pt1_1 = temp_select_lines[0].Line_now[0];
    cv::Point pt1_2 = temp_select_lines[0].Line_now[1];
    cv::Point pt2_1 = temp_select_lines[1].Line_now[0];
    cv::Point pt2_2 = temp_select_lines[1].Line_now[1];
    cv::Point tempPoint =
        CalcuateTools::GetInstance()->GetCrossPoint(pt1_1, pt1_2, pt2_1, pt2_2);
    //    cv::Point tempPoint = UpdateVanishingPoint(
    //        select_lane_lines, data_frame->adapt_vanishing_point);
    int tdis_1 = abs(tempPoint.x - data_frame->adapt_vanishing_point.x);
    int tdis_2 = abs(tempPoint.y - data_frame->adapt_vanishing_point.y);

    if (tempPoint.y < middle_start_line_ && tdis_1 < off_x_length &&
        tdis_2 < off_y_length) {
      tdis_1 = abs(tempPoint.x - data_frame->constant_vanishing_point.x);
      tdis_2 = abs(tempPoint.y - data_frame->constant_vanishing_point.y);
      if (tdis_1 < off_x_length / 2 && tdis_2 < off_y_length / 2) {
        INITLDVP tInitldvp;
        tInitldvp.x = tempPoint.x;
        tInitldvp.y = tempPoint.y;
        tInitldvp.left_point[0] = pt1_1;
        tInitldvp.left_point[1] = pt1_2;
        tInitldvp.right_point[0] = pt2_1;
        tInitldvp.right_point[1] = pt2_2;
        save_good_vps.push_back(tInitldvp);

        if (!data_frame->auto_calibration_flag &&
            save_good_vps.size() > data_frame_->Stable_Num *
                                       data_frame_->Update_Gap *
                                       data_frame_->Auto_Frame_Gap) {
          Point2f retVp = GetVanishingPointByLSM(save_good_vps);
          if (abs(retVp.x - m_user_setVP.x) < off_x_length &&
              abs(retVp.y - m_user_setVP.y) < off_y_length) {
            data_frame->UpdateInit(retVp.x, retVp.y);
            data_frame->auto_calibration_flag = false;
            save_good_vps.clear();
          }
        }
      } else {
        save_good_vps.clear();
      }
      data_frame->adapt_vanishing_point = tempPoint;
    } else if (tdis_1 > 2 * off_x_length || tdis_2 > 2 * off_y_length) {
      temp_select_lines.clear();
    }
    single_line_continue = 0;
  } else if (line_size == 1) {
    if (single_line_continue <
        data_frame_->Stable_Num * data_frame_->Auto_Frame_Gap) {
      for (int t = 0; t < select_lane_lines.size(); t++) {
        int center_offset =
            abs(temp_select_lines[0].pCenter - select_lane_lines[t].pCenter);
        int bottom_offset = abs(temp_select_lines[0].pCenterBottom -
                                select_lane_lines[t].pCenterBottom);
        if (temp_select_lines[0].nIsCurLane ==
                select_lane_lines[t].nIsCurLane &&
            center_offset < refine_lane_width_ / 3 &&
            bottom_offset < kRoiWidth / 4) {
          select_lane_lines[t] = temp_select_lines[0];
          temp_select_lines = select_lane_lines;
          break;
        }
      }
    } else {
      single_line_continue = 0;
    }
  }

  select_lane_lines = temp_select_lines;
  //  std::cout << "showLines:" << select_lane_lines.size() << ","
  //            << refine_lane_width_ << std::endl;
  //更新kalman滤波中 去掉车道中间的杂线
  lane_modefit_->lane_kalman_->ExcludeOutKalmanLinesByCurrent(
      select_lane_lines);
}

bool LaneDetection::ExcludeOutLineByExtendPoint(LaneLine& fit_spline) {
  cv::Point down_point = fit_spline.near_field[3];
  cv::Point up_point = fit_spline.middle_field[2];

  int off_x = abs(up_point.x - down_point.x);
  int off_y = abs(up_point.y - down_point.y);

  if (off_x > 2 * kImageWidth / 3 || off_y < kImageHeight / 3) {
    return false;
  }

  return true;
}

bool LaneDetection::ExcludeOutLinesInOneLine(LaneLine& _fitline) {
  if (_fitline.nQuality > kQualityLimit) return true;

  int deltaX_Min = kImageWidth / 20;
  int deltaX_Max = 1 * kImageWidth / 2;
  cv::Point pt0 = _fitline.Line_now[0];
  cv::Point pt1 = _fitline.Line_now[1];
  cv::Point pt2 = _fitline.Line_now[2];
  cv::Point pt3 = _fitline.Line_now[3];

  float fTop = _fitline.pCenterTop;
  float fCenter = _fitline.pCenter;
  float pCenterBottom = _fitline.pCenterBottom;

  cv::Point centerPt0 = data_frame_->virtual_center_lines.init[0];
  cv::Point centerPt1 = data_frame_->virtual_center_lines.init[1];
  cv::Point centerPt2 = data_frame_->virtual_center_lines.init[2];

  float tMinLaneWidth = refine_lane_width_;

  // keep visual center line
  if (data_frame_->virtual_center_lines.sflag ==
      kCurrentLineIndexLeftAndRight) {
    centerPt0 = data_frame_->virtual_center_lines.pixel[0];
    centerPt1 = data_frame_->virtual_center_lines.pixel[1];
    centerPt2 = data_frame_->virtual_center_lines.pixel[2];

    if (abs(centerPt0.x - pt1.x) > 2 * tMinLaneWidth ||
        abs(centerPt1.x - pt0.x) > tMinLaneWidth ||
        abs(centerPt2.x - pt2.x) > 2 * tMinLaneWidth)
      return false;
  } else {
    if (abs(centerPt0.x - pt1.x) > 2 * tMinLaneWidth ||
        abs(centerPt1.x - pt0.x) > 1.5 * tMinLaneWidth ||
        abs(centerPt2.x - pt2.x) > 2 * tMinLaneWidth)
      return false;
  }

  int ntempDis = abs(pt2.x - pt1.x);
  if ((/*abs(_fitline.pCenter) > gMinLaneWidth &&*/ ntempDis < deltaX_Min) ||
      ntempDis > deltaX_Max) {
    return false;
  }

  float fCenter_Bottom = _fitline.pCenter - _fitline.pCenterBottom;
  if (_fitline.pCenterBottom < 0 && fCenter_Bottom < -refine_lane_width_ / 2) {
    return false;
  } else if (_fitline.pCenterBottom > 0 &&
             fCenter_Bottom > refine_lane_width_ / 2) {
    return false;
  }

  return true;
}

bool LaneDetection::ExcludeOutLinesInMuliLineSingle(LaneLine& _fitline) {
  if (_fitline.nQuality > kQualityLimit) return true;

  if (_fitline.nIsCurLane <= kCurrentLineIndexLeft &&
      (_fitline.pCenter - _fitline.pCenterBottom) < -refine_lane_width_ / 3) {
    return false;
  } else if (_fitline.nIsCurLane >= kCurrentLineIndexRight &&
             (_fitline.pCenter - _fitline.pCenterBottom) >
                 refine_lane_width_ / 3) {
    return false;
  }

  return true;
}

std::vector<LaneLine> LaneDetection::ExcludeOutLineByPoint(
    std::vector<LaneLine>& _srcVecLine) {
  std::vector<LaneLine> resVecLines;

  int vanPointLenX = data_frame_->x_Length;
  int vanPointLenY = data_frame_->y_Length;

  float adaptValue = vanPointLenY / 2;
  int nLen = _srcVecLine.size();
  bool* pIsUse = new bool[nLen];
  bool* fDis = new bool[nLen];
  for (int i = 0; i < nLen; i++) {
    pIsUse[i] = false;
    fDis[i] = false;
  }

  cv::Point2f ptB = cv::Point2f(-1, -1);
  cv::Point2f ptT = cv::Point2f(-1, -1);

  cv::Point inVanishingPoint = data_frame_->adapt_vanishing_point;

  float nExtendSize = 1;
  float maxX = inVanishingPoint.x + nExtendSize * vanPointLenX;
  float minX = inVanishingPoint.x - nExtendSize * vanPointLenX;
  float maxY = inVanishingPoint.y + nExtendSize * vanPointLenY;
  float minY = inVanishingPoint.y - nExtendSize * vanPointLenY;

  float nExtendSize_g = 1.2;
  inVanishingPoint = data_frame_->constant_vanishing_point;
  float maxX_g = inVanishingPoint.x + nExtendSize_g * vanPointLenX;
  float minX_g = inVanishingPoint.x - nExtendSize_g * vanPointLenX;
  float maxY_g = inVanishingPoint.y + nExtendSize_g * vanPointLenY;
  float minY_g = inVanishingPoint.y - nExtendSize_g * vanPointLenY;

  for (int i = 0; i < nLen; i++) {
    if (pIsUse[i] == false) {
      LaneLine fitLineI = _srcVecLine[i];
      for (int k = i + 1; k < nLen; k++) {
        LaneLine fitLineK = _srcVecLine[k];
        ptB = CalcuateTools::GetInstance()->GetCrossPoint(
            fitLineI.Line_now[0], fitLineI.Line_now[1], fitLineK.Line_now[0],
            fitLineK.Line_now[1]);
        ptT = CalcuateTools::GetInstance()->GetCrossPoint(
            fitLineI.Line_now[2], fitLineI.Line_now[3], fitLineK.Line_now[2],
            fitLineK.Line_now[3]);

        if ((ptB.x < maxX && ptB.x > minX) && (ptB.y < maxY && ptB.y > minY) &&
            (ptT.x < maxX && ptT.x > minX) && (ptT.y < maxY && ptT.y > minY)) {
          pIsUse[i] = true;
          pIsUse[k] = true;
        } else if (/*(ptB.x < maxX_g && ptB.x > minX_g) && */ (ptB.y < maxY_g &&
                                                               ptB.y > minY_g)
                   /*&& (ptT.x < maxX_g && ptT.x > minX_g)*/ &&
                   (ptT.y < maxY_g && ptT.y > minY_g)) {
          pIsUse[i] = true;
          pIsUse[k] = true;
        }
      }

      if (pIsUse[i] == false) {
        double disB = CalcuateTools::GetInstance()->PointToLineDistByPoint(
            data_frame_->adapt_vanishing_point, _srcVecLine[i].Line_now[0],
            _srcVecLine[i].Line_now[1]);
        double disT = CalcuateTools::GetInstance()->PointToLineDistByPoint(
            data_frame_->adapt_vanishing_point, _srcVecLine[i].Line_now[2],
            _srcVecLine[i].Line_now[3]);

        if (disB < adaptValue || disT < adaptValue) {
          fDis[i] = true;
        }

        disB = CalcuateTools::GetInstance()->PointToLineDistByPoint(
            data_frame_->constant_vanishing_point, _srcVecLine[i].Line_now[0],
            _srcVecLine[i].Line_now[1]);
        disT = CalcuateTools::GetInstance()->PointToLineDistByPoint(
            data_frame_->constant_vanishing_point, _srcVecLine[i].Line_now[2],
            _srcVecLine[i].Line_now[3]);
        if (disB < adaptValue || disT < adaptValue) {
          fDis[i] = true;
        }
      }
    }
  }

  for (int t = 0; t < nLen; t++) {
    if (pIsUse[t] || fDis[t]) resVecLines.push_back(_srcVecLine[t]);
  }

  delete[] pIsUse;
  delete[] fDis;

  return resVecLines;
}

std::vector<LaneLine> LaneDetection::ExcludeOutLines(
    std::vector<LaneLine>& lane_lines) {
  std::vector<LaneLine> resVecLine;
  int nSize = lane_lines.size();
  if (nSize < 1) {
    return resVecLine;
  }
  int nCenterX = kImageWidth / 2;
  //  int nCenterX = data_frame_->constant_vanishing_point.x / 2;

  float mScale = 1.5f;

  for (int i = 0; i < nSize; ++i) {
    lane_lines[i].pCenterTop = lane_lines[i].Line_now[2].x - nCenterX;
    lane_lines[i].pCenter = lane_lines[i].Line_now[0].x - nCenterX;
    lane_lines[i].pCenterBottom = lane_lines[i].Line_now[1].x - nCenterX;
    lane_lines[i].bIsTrue = true;
    lane_lines[i].nIsCurLane = 0;
  }

  std::sort(lane_lines.begin(), lane_lines.end(), SortBypCenterBottom);

  //  resVecLine = ExcludeOutLinesByJl(lane_lines, mScale);

  resVecLine = ExcludeOutLinesMain(lane_lines, mScale);

  if (resVecLine.size() < 2) {
    const_roi_continue = std::max((short)0, --const_roi_continue);
  } else {
    const_roi_continue =
        std::min(data_frame_->Stable_Num, ++const_roi_continue);
  }

  return resVecLine;
}

std::vector<LaneLine> LaneDetection::ExcludeOutLinesMain(
    std::vector<LaneLine>& lane_lines, float mScale) {
  std::vector<LaneLine> reultVecLine;
  int nSize = lane_lines.size();
  if (nSize < 1) {
    return reultVecLine;
  }

  const int only_left = -1;
  const int only_right = -2;

  int k = 0;
  if (nSize == 1 && lane_lines[0].pCenterBottom < 0) {
    k = only_left;
  } else if (nSize == 1 && lane_lines[0].pCenterBottom > 0) {
    k = only_right;
  } else {
    for (int i = 1; i < nSize; ++i) {
      if (lane_lines[0].pCenterBottom > 0 && lane_lines[1].pCenterBottom > 0) {
        k = only_right;
        break;
      } else if (lane_lines[0].pCenterBottom < 0 &&
                 lane_lines[nSize - 1].pCenterBottom < 0) {
        k = only_left;
        break;
      }

      if (lane_lines[i - 1].pCenterBottom < 0 &&
          lane_lines[i].pCenterBottom > 0) {
        k = i;
        break;
      }
    }
  }

  switch (k) {
    case only_left: {
      if (abs(lane_lines[nSize - 1].pCenter) > mScale * refine_lane_width_) {
        break;
      }

      int left_offset_center = kUnknowValue;

      int select_index = nSize - 1;
      if (current_show_lines.bflag &&
          current_show_lines.left_location != kUnknowValue) {
        select_index = GetSelectLine(lane_lines, false, left_offset_center);
      }
      if (select_index >= 0 && select_index < nSize) {
        lane_lines[select_index].nIsCurLane = kCurrentLineIndexLeft;
        current_show_lines.bflag = true;
        current_show_lines.left_index = lane_lines[select_index].nIndex;
        current_show_lines.left_location = lane_lines[select_index].pCenter;
        current_show_lines.left_bottom = lane_lines[select_index].pCenterBottom;
        current_show_lines.right_location = kUnknowValue;
        current_show_lines.right_bottom = kUnknowValue;

        reultVecLine.push_back(lane_lines[select_index]);
        // add next lanes...
        //...........
      }

    } break;
    case only_right: {
      if (abs(lane_lines[0].pCenter) > mScale * refine_lane_width_) {
        break;
      }

      int right_offset_center = kUnknowValue;

      int select_index = 0;
      if (current_show_lines.bflag &&
          current_show_lines.right_location != kUnknowValue) {
        select_index = GetSelectLine(lane_lines, true, right_offset_center);
      }
      if (select_index >= 0 && select_index < nSize) {
        lane_lines[select_index].nIsCurLane = kCurrentLineIndexRight;
        current_show_lines.bflag = true;
        current_show_lines.right_index = lane_lines[select_index].nIndex;
        current_show_lines.right_location = lane_lines[select_index].pCenter;
        current_show_lines.right_bottom =
            lane_lines[select_index].pCenterBottom;
        current_show_lines.left_location = kUnknowValue;
        current_show_lines.left_bottom = kUnknowValue;

        reultVecLine.push_back(lane_lines[select_index]);
        // add next lanes...
        //...........
      }

    } break;
    default: {
      if (k < 0 || k > nSize - 1) {
        break;
      }

      int left_offset_center = kUnknowValue;
      int right_offset_center = kUnknowValue;
      int left_finally_index = -1;
      int right_finally_index = -1;
      int left_count = 1;
      int right_count = 0;
      int left_max_quality = 0;
      int right_max_quality = 0;

      // select left
      int nSelectIndex = -1;
      if (current_show_lines.bflag &&
          current_show_lines.left_location != kUnknowValue) {
        nSelectIndex = GetSelectLine(lane_lines, false, left_offset_center);
      }
      if (nSelectIndex != -1) {
        left_finally_index = nSelectIndex;
      } else /*if (const_roi_continue < 2)*/ {
        while (k - left_count >= 0) {
          if (abs(lane_lines[k - left_count].pCenter) <
              mScale * refine_lane_width_) {
            int t_quality = lane_lines[k - left_count].nQuality;
            if (t_quality >= kQualityLimit &&
                left_max_quality >= kQualityLimit) {
              left_count++;
              continue;
            } else if (t_quality > left_max_quality) {
              left_max_quality = t_quality;
              left_finally_index = k - left_count;
            }
          } else {
            break;
          }
          left_count++;
        }

        //        left_offset_center = ;
      }

      nSelectIndex = -1;
      if (current_show_lines.bflag &&
          current_show_lines.right_location != kUnknowValue) {
        nSelectIndex = GetSelectLine(lane_lines, true, right_offset_center);
      }
      if (nSelectIndex != -1) {
        right_finally_index = nSelectIndex;
      } else /*if (const_roi_continue < 2)*/ {
        while (k + right_count < nSize) {
          if (abs(lane_lines[k + right_count].pCenter) <
              mScale * refine_lane_width_) {
            int t_quality = lane_lines[k + right_count].nQuality;
            if (t_quality >= kQualityLimit &&
                right_max_quality >= kQualityLimit) {
              right_count++;
              continue;
            } else if (t_quality > right_max_quality) {
              right_max_quality = t_quality;
              right_finally_index = k + right_count;
            }
          } else {
            break;
          }
          right_count++;
        }
      }

      if (left_finally_index != -1 && right_finally_index != -1) {
        float dis_center = abs(lane_lines[left_finally_index].pCenter -
                               lane_lines[right_finally_index].pCenter);
        float extend_k = 1.5;
        if (const_roi_continue > 0) {
          extend_k = 1.25;
        }
        if (dis_center < refine_lane_width_ / extend_k ||
            dis_center > refine_lane_width_ * extend_k) {
          if (left_max_quality > right_max_quality) {
            lane_lines[right_finally_index].bIsTrue = false;
            right_finally_index = -1;
          } else {
            lane_lines[left_finally_index].bIsTrue = false;
            left_finally_index = -1;
          }
        }
      }

      bool suc_left = false;
      bool suc_right = false;
      int left_number = -1;
      int right_number = -1;

      if (right_finally_index == -1 && left_finally_index >= 0 &&
          left_finally_index < nSize) {
        lane_lines[left_finally_index].nIsCurLane = kCurrentLineIndexLeft;
        current_show_lines.bflag = true;
        current_show_lines.left_index = lane_lines[left_finally_index].nIndex;
        current_show_lines.left_location =
            lane_lines[left_finally_index].pCenter;
        current_show_lines.left_bottom =
            lane_lines[left_finally_index].pCenterBottom;
        current_show_lines.right_location = kUnknowValue;
        current_show_lines.right_bottom = kUnknowValue;

        reultVecLine.push_back(lane_lines[left_finally_index]);
        suc_left = true;
        left_number = 1;
      }
      if (left_finally_index == -1 && right_finally_index >= 0 &&
          right_finally_index < nSize) {
        lane_lines[right_finally_index].nIsCurLane = kCurrentLineIndexRight;
        current_show_lines.right_index = lane_lines[right_finally_index].nIndex;
        current_show_lines.bflag = true;
        current_show_lines.right_location =
            lane_lines[right_finally_index].pCenter;
        current_show_lines.right_bottom =
            lane_lines[right_finally_index].pCenterBottom;
        current_show_lines.left_location = kUnknowValue;
        current_show_lines.left_bottom = kUnknowValue;

        reultVecLine.push_back(lane_lines[right_finally_index]);
        suc_right = true;
        right_number = 1;
      } else if (left_finally_index >= 0 && left_finally_index < nSize &&
                 right_finally_index >= 0 && right_finally_index < nSize) {
        lane_lines[left_finally_index].nIsCurLane = kCurrentLineIndexLeft;
        lane_lines[right_finally_index].nIsCurLane = kCurrentLineIndexRight;
        current_show_lines.left_index = lane_lines[left_finally_index].nIndex;
        current_show_lines.right_index = lane_lines[right_finally_index].nIndex;
        current_show_lines.bflag = true;
        current_show_lines.left_location =
            lane_lines[left_finally_index].pCenter;
        current_show_lines.left_bottom =
            lane_lines[left_finally_index].pCenterBottom;
        reultVecLine.push_back(lane_lines[left_finally_index]);
        suc_left = true;
        left_number = 1;

        current_show_lines.right_location =
            lane_lines[right_finally_index].pCenter;
        current_show_lines.right_bottom =
            lane_lines[right_finally_index].pCenterBottom;

        reultVecLine.push_back(lane_lines[right_finally_index]);
        suc_right = true;
        right_number = 1;
      } else {
        current_show_lines.right_location = kUnknowValue;
        current_show_lines.left_location = kUnknowValue;
        break;
      }

    } break;
  }

  return reultVecLine;
}

std::vector<LaneLine> LaneDetection::ExcludeOutLinesByJl(
    std::vector<LaneLine>& lane_lines, float mScale) {
  std::vector<LaneLine> reultVecLine;
  int nSize = lane_lines.size();
  if (nSize < 1) {
    return reultVecLine;
  }

  int nLeft = -1;
  int nRight = -1;

  int k = 0;
  if (nSize == 1 && lane_lines[0].pCenterBottom < 0) {
    k = -2;
  }
  if (nSize == 1 && lane_lines[0].pCenterBottom > 0) {
    k = -1;
  }

  for (int i = 1; i < nSize; ++i) {
    if (lane_lines[0].pCenterBottom > 0 && lane_lines[1].pCenterBottom > 0) {
      k = -1;
      break;
    } else if (lane_lines[0].pCenterBottom < 0 &&
               lane_lines[nSize - 1].pCenterBottom < 0) {
      k = -2;
      break;
    }

    if (lane_lines[i - 1].pCenterBottom < 0 &&
        lane_lines[i].pCenterBottom > 0) {
      k = i;
      break;
    }
  }

  if (k == -1) {
    if (abs(lane_lines[0].pCenter) > mScale * refine_lane_width_) {
      return reultVecLine;
    }

    int nSelectRightIndex = 0;
    if (current_show_lines.bflag &&
        current_show_lines.left_location != kUnknowValue) {
      int retIndex =
          GetDatumLine(lane_lines, current_show_lines.left_location, false);
      if (retIndex != -1) {
        nSelectRightIndex = retIndex;
      }
    }

    lane_lines[nSelectRightIndex].nIsCurLane = kCurrentLineIndexRight;
    current_show_lines.right_location = lane_lines[nSelectRightIndex].pCenter;

    reultVecLine.push_back(lane_lines[nSelectRightIndex]);
    nRight = 1;
    for (int i = nSelectRightIndex + 1; i < nSize; ++i) {
      float fDis =
          abs(reultVecLine[nRight - 1].pCenter - lane_lines[i].pCenter);
      if (fDis > refine_lane_width_ / 2 && fDis < mScale * refine_lane_width_) {
        reultVecLine.push_back(lane_lines[i]);
        nRight++;
      }
      if (nRight > kRefineMaxRightCount - 1) {
        break;
      }
    }
    current_show_lines.left_location = kUnknowValue;
  } else if (k == -2) {
    if (abs(lane_lines[nSize - 1].pCenter) > mScale * refine_lane_width_) {
      return reultVecLine;
    }

    int nSelectLeftIndex = nSize - 1;
    if (current_show_lines.bflag &&
        current_show_lines.left_location != kUnknowValue) {
      int retIndex =
          GetDatumLine(lane_lines, current_show_lines.left_location, true);
      if (retIndex != -1) {
        nSelectLeftIndex = retIndex;
      }
    }

    lane_lines[nSelectLeftIndex].nIsCurLane = kCurrentLineIndexLeft;
    current_show_lines.left_location = lane_lines[nSelectLeftIndex].pCenter;

    reultVecLine.push_back(lane_lines[nSelectLeftIndex]);
    nLeft = 1;
    for (int i = nSelectLeftIndex - 1; i >= 0; --i) {
      float fDis = abs(reultVecLine[nLeft - 1].pCenter - lane_lines[i].pCenter);
      if (fDis > refine_lane_width_ / 2 && fDis < mScale * refine_lane_width_) {
        reultVecLine.push_back(lane_lines[i]);
        nLeft++;
      }
      if (nLeft > kRefineMaxLeftCount - 1) {
        break;
      }
    }
    current_show_lines.right_location = kUnknowValue;
  } else if (k > 0 && k < nSize) {
    int nLeftMaxIndex = -1;
    int nRightMaxIndex = -1;

    int nLeftMaxQuality = 0;
    int nRightMaxQuality = 0;
    int nLeftMaxVecSize = -1;
    int nRightMaxVecSize = -1;
    int nLeftIndex = 1;
    int nRightIndex = 0;

    int nSelectIndex = -1;
    if (current_show_lines.bflag &&
        current_show_lines.left_location != kUnknowValue) {
      nSelectIndex =
          GetDatumLine(lane_lines, current_show_lines.left_location, false);
    }
    if (nSelectIndex != -1) {
      nLeftMaxIndex = nSelectIndex;
    } else {
      while (k - nLeftIndex >= 0 /*&& nLeftIndex < 3*/) {
        if (abs(lane_lines[k - nLeftIndex].pCenter) <
            mScale * refine_lane_width_) {
          // int nLeftSize = lane_lines[k - nLeftIndex].vecPointList.size();
          int nLeftSize = lane_lines[k - nLeftIndex].nDistance[0] +
                          lane_lines[k - nLeftIndex].nDistance[1];
          int nLeftQuality = lane_lines[k - nLeftIndex].nQuality;
          if (nLeftQuality >= kQualityLimit &&
              nLeftMaxQuality >= kQualityLimit) {
            nLeftIndex++;
            continue;
          } else if (nLeftQuality > nLeftMaxQuality ||
                     (nLeftQuality == nLeftMaxQuality &&
                      nLeftSize > nLeftMaxVecSize)) {
            nLeftMaxVecSize = nLeftSize;
            nLeftMaxQuality = nLeftQuality;
            nLeftMaxIndex = k - nLeftIndex;
          }
        } else {
          break;
        }

        nLeftIndex++;
      }
    }

    nSelectIndex = -1;
    if (current_show_lines.bflag &&
        current_show_lines.right_location != kUnknowValue) {
      nSelectIndex =
          GetDatumLine(lane_lines, current_show_lines.right_location, true);
    }

    if (nSelectIndex != -1) {
      nRightMaxIndex = nSelectIndex;
    } else {
      while (k + nRightIndex < nSize /*&& nRightIndex < 2*/) {
        if (abs(lane_lines[k + nRightIndex].pCenter) <
            mScale * refine_lane_width_) {
          // int nRightSize = lane_lines[k + nRightIndex].vecPointList.size();
          int nRightSize = lane_lines[k + nRightIndex].nDistance[0] +
                           lane_lines[k + nRightIndex].nDistance[1];
          int nRightQuality = lane_lines[k + nRightIndex].nQuality;
          if (nRightQuality >= kQualityLimit &&
              nRightMaxQuality >= kQualityLimit) {
            nRightIndex++;
            continue;
          } else if (nRightQuality > nRightMaxQuality ||
                     (nRightQuality == nRightMaxQuality &&
                      nRightSize > nRightMaxVecSize)) {
            nRightMaxVecSize = nRightSize;
            nRightMaxQuality = nRightQuality;
            nRightMaxIndex = k + nRightIndex;
          }
        } else {
          break;
        }
        nRightIndex++;
      }
    }

    int nCurLeftLaneIndex = nLeftMaxIndex;
    int nCurRightLaneIndex = nRightMaxIndex;
    short sErrorType = -1;
    float sMaxGapT = 1.25f * mScale * refine_lane_width_;

    if (nLeftMaxIndex != -1 && nRightMaxIndex != -1) {
      float ntempDis = abs(lane_lines[nRightMaxIndex].pCenter -
                           lane_lines[nLeftMaxIndex].pCenter);
      if (ntempDis < refine_lane_width_ / 2) {
        sErrorType = 1;  // min
      } else if (ntempDis > sMaxGapT) {
        sErrorType = 2;  // max
      }

      if ((lane_lines[nLeftMaxIndex].pCenter > 0 &&
           lane_lines[nLeftMaxIndex].pCenter >
               lane_lines[nRightMaxIndex].pCenter) ||
          (lane_lines[nRightMaxIndex].pCenter < 0 &&
           lane_lines[nLeftMaxIndex].pCenter >
               lane_lines[nRightMaxIndex].pCenter)) {
        sErrorType = 3;
      }

      if (sErrorType != -1) {
        int bRunRemove = true;
        if (current_show_lines.bflag) {
          float fDisDatumLine = 0.2 * refine_lane_width_;
          int nDisDatumLine_right = abs(current_show_lines.right_location -
                                        lane_lines[nRightMaxIndex].pCenter);
          int nDisDatumLine_left = abs(current_show_lines.left_location -
                                       lane_lines[nLeftMaxIndex].pCenter);
          if (nDisDatumLine_right > fDisDatumLine &&
              nDisDatumLine_left <= fDisDatumLine) {
            lane_lines[nRightMaxIndex].bIsTrue = false;
            nCurRightLaneIndex = -1;
            bRunRemove = false;
          } else if (nDisDatumLine_right <= fDisDatumLine &&
                     nDisDatumLine_left > fDisDatumLine) {
            lane_lines[nLeftMaxIndex].bIsTrue = false;
            nCurLeftLaneIndex = -1;
            bRunRemove = false;
          }
        }

        if (bRunRemove) {
          if (nLeftMaxQuality > nRightMaxQuality ||
              (nLeftMaxQuality == nRightMaxQuality &&
               nLeftMaxVecSize > nRightMaxVecSize)) {
            lane_lines[nRightMaxIndex].bIsTrue = false;
            nCurRightLaneIndex = -1;
          } else {
            lane_lines[nLeftMaxIndex].bIsTrue = false;
            nCurLeftLaneIndex = -1;
          }
        }
      }
    }

    int nLeftPriCenterValue = 0;
    int nRightPriCenterValue = 0;
    bool bSucLeft = false;
    bool bSucRight = false;

    // 2017.7.21
    if (nCurRightLaneIndex == -1 && nCurLeftLaneIndex >= 0 &&
        nCurLeftLaneIndex < nSize) {
      int nConMaxQuality = -1;
      for (int k = nCurLeftLaneIndex + 1; k < nSize; ++k) {
        if (lane_lines[k].pCenter >= 0) {
          int nLRdis = abs(lane_lines[k].pCenter -
                           lane_lines[nCurLeftLaneIndex].pCenter);
          if (nLRdis > refine_lane_width_ && lane_lines[k].bIsTrue) {
            if (nConMaxQuality < lane_lines[k].nQuality) {
              nCurRightLaneIndex = k;
              nConMaxQuality = lane_lines[k].nQuality;
            }
          }
        }
      }
    }

    if (nCurLeftLaneIndex == -1 && nCurRightLaneIndex >= 0 &&
        nCurRightLaneIndex < nSize) {
      int nConMaxQuality = -1;
      for (int k = nCurRightLaneIndex - 1; k >= 0; --k) {
        if (lane_lines[k].pCenter <= 0) {
          int nLRdis = abs(lane_lines[k].pCenter -
                           lane_lines[nCurRightLaneIndex].pCenter);
          if (nLRdis > refine_lane_width_ && lane_lines[k].bIsTrue) {
            if (nConMaxQuality < lane_lines[k].nQuality) {
              nCurLeftLaneIndex = k;
              nConMaxQuality = lane_lines[k].nQuality;
            }
          }
        }
      }
    }

    if (nCurRightLaneIndex == -1 && nCurLeftLaneIndex >= 0 &&
        nCurLeftLaneIndex < nSize) {
      lane_lines[nCurLeftLaneIndex].nIsCurLane = kCurrentLineIndexLeft;

      current_show_lines.left_location = lane_lines[nCurLeftLaneIndex].pCenter;
      current_show_lines.right_location = kUnknowValue;

      reultVecLine.push_back(lane_lines[nCurLeftLaneIndex]);
      nLeftPriCenterValue = lane_lines[nCurLeftLaneIndex].pCenter;
      bSucLeft = true;
      nLeft = 1;
    } else if (nCurLeftLaneIndex == -1 && nCurRightLaneIndex >= 0 &&
               nCurRightLaneIndex < nSize) {
      lane_lines[nCurRightLaneIndex].nIsCurLane = kCurrentLineIndexRight;

      current_show_lines.right_location =
          lane_lines[nCurRightLaneIndex].pCenter;
      current_show_lines.left_location = kUnknowValue;

      reultVecLine.push_back(lane_lines[nCurRightLaneIndex]);
      nRightPriCenterValue = lane_lines[nCurRightLaneIndex].pCenter;
      bSucRight = true;
      nRight = 1;
    } else if (nCurRightLaneIndex >= 0 && nCurRightLaneIndex < nSize &&
               nCurLeftLaneIndex >= 0 && nCurLeftLaneIndex < nSize) {
      lane_lines[nCurLeftLaneIndex].nIsCurLane = kCurrentLineIndexLeft;
      lane_lines[nCurRightLaneIndex].nIsCurLane = kCurrentLineIndexRight;

      current_show_lines.left_location = lane_lines[nCurLeftLaneIndex].pCenter;
      current_show_lines.right_location =
          lane_lines[nCurRightLaneIndex].pCenter;

      reultVecLine.push_back(lane_lines[nCurLeftLaneIndex]);
      reultVecLine.push_back(lane_lines[nCurRightLaneIndex]);
      nLeftPriCenterValue = lane_lines[nCurLeftLaneIndex].pCenter;
      nRightPriCenterValue = lane_lines[nCurRightLaneIndex].pCenter;

      nLeft = 1;
      nRight = 1;
      bSucLeft = true;
      bSucRight = true;
    } else {
      current_show_lines.right_location = kUnknowValue;
      current_show_lines.left_location = kUnknowValue;
      return reultVecLine;
    }

    float fMinLaneWidth = refine_lane_width_;
    fMinLaneWidth =
        fMinLaneWidth > kImageWidth / 10 ? fMinLaneWidth : kImageWidth / 10;
    fMinLaneWidth =
        fMinLaneWidth < kImageWidth / 2 ? fMinLaneWidth : kImageWidth / 2;

    if (bSucLeft) {
      for (int i = nCurLeftLaneIndex - 1; i >= 0; --i) {
        if (nLeft > kRefineMaxLeftCount - 1) {
          break;
        }
        float tDis = abs(lane_lines[i].pCenter - nLeftPriCenterValue);
        if (lane_lines[i].bIsTrue && tDis > fMinLaneWidth &&
            tDis < mScale * fMinLaneWidth) {
          lane_lines[i].nIsCurLane = kCurrentLineIndexLeft - nLeft;
          reultVecLine.push_back(lane_lines[i]);
          nLeftPriCenterValue = lane_lines[i].pCenter;
          nLeft++;
        }
      }
    }
    if (bSucRight) {
      for (int i = nCurRightLaneIndex + 1; i < nSize; ++i) {
        if (nRight > kRefineMaxRightCount - 1) {
          break;
        }
        float tDis = abs(lane_lines[i].pCenter - nRightPriCenterValue);
        if (lane_lines[i].bIsTrue && tDis > fMinLaneWidth &&
            tDis < mScale * fMinLaneWidth
            /*&& abs(lane_lines[i].pCenter - nRightPriCenterValue)  > 300*/) {
          lane_lines[i].nIsCurLane = kCurrentLineIndexRight + nRight;
          reultVecLine.push_back(lane_lines[i]);
          nRightPriCenterValue = lane_lines[i].pCenter;
          nRight++;
        }
      }
    }
  }
  return reultVecLine;
}

int LaneDetection::GetSelectLine(std::vector<LaneLine>& lane_lines,
                                 bool is_right, int& offset_center) {
  float left_top = current_show_lines.left_top;
  float right_top = current_show_lines.right_top;
  float left_location = current_show_lines.left_location;
  float right_location = current_show_lines.right_location;
  float left_bottom = current_show_lines.left_bottom;
  float right_bottom = current_show_lines.right_bottom;
  float left_rho = current_show_lines.left_rho;
  float right_rho = current_show_lines.right_rho;
  long left_index = current_show_lines.left_index;
  long right_index = current_show_lines.right_index;

  float kTheth = kMinWidthScale;
  int nSize = lane_lines.size();
  int index = -1;
  float pMinCenter = kUnknowValue;
  int nMaxQuality = -1;
  for (int i = 0; i < nSize; ++i) {
    float refer_location = left_location;
    float refer_bottom = left_bottom;
    long refer_index = left_index;

    if (is_right) {
      refer_location = right_location;
      refer_bottom = right_bottom;
      refer_index = right_index;
      if (lane_lines[i].pCenterBottom < 0) continue;
    } else if (!is_right && lane_lines[i].pCenterBottom > 0) {
      continue;
    }
    float abs_location = abs(lane_lines[i].pCenter - refer_location);
    float abs_bottom = abs(lane_lines[i].pCenterBottom - refer_bottom);

    if (abs_location < kTheth * refine_lane_width_) {
      if (refer_index == lane_lines[i].nIndex) {
        index = i;
        break;
      }
      float abs_sum = abs_location + 0.5 * abs_bottom;
      if (nMaxQuality < lane_lines[i].nQuality ||
          ((nMaxQuality == lane_lines[i].nQuality) && pMinCenter < abs_sum)) {
        pMinCenter = abs_sum;
        nMaxQuality = lane_lines[i].nQuality;
        index = i;
      }
    }
  }
  //  std::cout << "GetSelectLine:" << index << "," << lane_lines[index].nIndex
  //            << std::endl;
  offset_center = pMinCenter;
  return index;
}

int LaneDetection::GetDatumLine(std::vector<LaneLine>& lane_lines,
                                float refer_point_x, bool is_right) {
  float kTheth = 0.2f;
  int nSize = lane_lines.size();
  int index = -1;
  float pMinCenter = kUnknowValue;
  int nMaxQuality = -1;
  for (int i = 0; i < nSize; ++i) {
    if (is_right && lane_lines[i].pCenterBottom < 0) {
      continue;
    } else if (!is_right && lane_lines[i].pCenterBottom > 0) {
      continue;
    }
    float tMinCenter = abs(lane_lines[i].pCenter - refer_point_x);
    if (tMinCenter < kTheth * refine_lane_width_) {
      if (nMaxQuality < lane_lines[i].nQuality ||
          ((nMaxQuality == lane_lines[i].nQuality) &&
           pMinCenter < tMinCenter)) {
        pMinCenter = tMinCenter;
        nMaxQuality = lane_lines[i].nQuality;
        index = i;
      }
    }
  }
  return index;
}

cv::Point LaneDetection::UpdateVanishingPoint(
    std::vector<LaneLine>& lane_lines, cv::Point current_vanishing_point) {
  cv::Point tempPoint = current_vanishing_point;

  int nSize = lane_lines.size();
  if (nSize == 0) {
    return tempPoint;
  }

  short nMax1 = -1;
  short nIndex1 = -1;
  short nMax2 = -1;
  short nIndex2 = -1;

  for (int i = 0; i < nSize; ++i) {
    short tempQuality = lane_lines[i].nQuality;

    if (tempQuality < kRefineQuality) {
      continue;
    }

    if (lane_lines[i].pCenterBottom > 0) {
      if (nMax1 < tempQuality) {
        nMax1 = tempQuality;
        nIndex1 = i;
      }
    } else {
      if (nMax2 < tempQuality) {
        nMax2 = tempQuality;
        nIndex2 = i;
      }
    }
  }

  if ((nIndex1 > -1 && nIndex1 < nSize) && nIndex2 > -1 && nIndex2 < nSize) {
    cv::Point buuPt1_1 = lane_lines[nIndex1].near_field[2];
    cv::Point buuPt1_2 = lane_lines[nIndex1].near_field[3];
    cv::Point buuPt2_1 = lane_lines[nIndex1].middle_field[2];
    cv::Point buuPt2_2 = lane_lines[nIndex1].middle_field[3];
    tempPoint = CalcuateTools::GetInstance()->GetCrossPoint(buuPt1_1, buuPt1_2,
                                                            buuPt2_1, buuPt2_2);
  }

  return tempPoint;
}

void LaneDetection::UpdateVisualCenterPoint(
    std::vector<LaneLine>& _srcVecLine) {
  int nSize = _srcVecLine.size();

  LaneLine leftSpline;
  LaneLine rightSpline;
  bool bLeft = false;
  bool bRight = false;

  for (int i = 0; i < nSize; ++i) {
    if (_srcVecLine[i].nIsCurLane == kCurrentLineIndexLeft) {
      bLeft = true;
      leftSpline = _srcVecLine[i];
    } else if (_srcVecLine[i].nIsCurLane == kCurrentLineIndexRight) {
      bRight = true;
      rightSpline = _srcVecLine[i];
    }
  }

  if (bLeft && bRight) {
    data_frame_->virtual_center_lines.sflag = kCurrentLineIndexLeftAndRight;

    data_frame_->virtual_center_lines.pixel[0].x =
        (leftSpline.Line_now[1].x + rightSpline.Line_now[1].x) / 2;
    data_frame_->virtual_center_lines.pixel[0].y =
        (leftSpline.Line_now[1].y + rightSpline.Line_now[1].y) / 2;
    data_frame_->virtual_center_lines.pixel[1].x =
        (leftSpline.Line_now[0].x + rightSpline.Line_now[0].x) / 2;
    data_frame_->virtual_center_lines.pixel[1].y =
        (leftSpline.Line_now[0].y + rightSpline.Line_now[0].y) / 2;
    data_frame_->virtual_center_lines.pixel[2].x =
        (leftSpline.Line_now[2].x + rightSpline.Line_now[2].x) / 2;
    data_frame_->virtual_center_lines.pixel[2].y =
        (leftSpline.Line_now[2].y + rightSpline.Line_now[2].y) / 2;

    memcpy(data_frame_->virtual_center_lines.init,
           data_frame_->virtual_center_lines.pixel,
           sizeof(data_frame_->virtual_center_lines.pixel));

  } else
    data_frame_->virtual_center_lines.sflag = 0;
}

void LaneDetection::TestRefineResult(std::vector<LaneLine>& line_results) {
  cv::Mat showPriSaveLine;
  process_image_.copyTo(showPriSaveLine);

  for (int i = 0; i < line_results.size(); ++i) {
    int nId = line_results[i].nIndex;
    cv::Point UpPointNow =
        line_results[i].Line_now[0];  // Line_now  Line_pre Line_current
    cv::Point DownPointNow = line_results[i].Line_now[1];
    cv::line(showPriSaveLine, cv::Point(UpPointNow.x, UpPointNow.y),
             cv::Point(DownPointNow.x, DownPointNow.y),
             data_frame_->scalar_color[nId % 7], 2,
             CV_AA);  // nId%7

    UpPointNow = line_results[i].Line_now[2];
    DownPointNow = line_results[i].Line_now[3];

    cv::line(showPriSaveLine, cv::Point(UpPointNow.x, UpPointNow.y),
             cv::Point(DownPointNow.x, DownPointNow.y),
             data_frame_->scalar_color[nId % 7], 2, CV_AA);
    cv::Point indexPt = cv::Point(DownPointNow.x, DownPointNow.y);
    // if (SHOW_INDEX)
    DebugShow::Instance()->ShowTextinMat(showPriSaveLine, indexPt, nId);
  }
  // debugFrameAssociation(showPriSaveLine, g_picture_ID, pBaseDir,
  // SHOW_WRITER_NINDEX);
  cv::imshow("TestRefineResult", showPriSaveLine);
}

void LaneDetection::TestShowLineType(std::vector<LaneLine>& line_results) {
  cv::Mat showImage_Result;
  process_image_.copyTo(showImage_Result);

  for (int i = 0; i < line_results.size(); i++) {
    int nId = line_results[i].nIndex;

    int showColorIndex = 6;
    int thickness = 3;
    int lineType = 0;

    int tmplineType = line_results[i].nLineType;

    //            if(showResPriSaveLine[i].nIsCurLane != 0)
    //                lineType = 1;

    if (tmplineType == 1) {
      lineType = 1;
    } else if (tmplineType == 2) {
      showColorIndex = 7;
    } else if (tmplineType == 3) {
      showColorIndex = 7;
      lineType = 1;
    }

    cv::Point UpPointNow = line_results[i].Line_now[0];
    cv::Point DownPointNow = line_results[i].Line_now[1];
    DebugShow::Instance()->zyrLine(showImage_Result, UpPointNow, DownPointNow,
                                   data_frame_->scalar_color[showColorIndex],
                                   thickness, lineType);

    UpPointNow = line_results[i].Line_now[2];
    DownPointNow = line_results[i].Line_now[3];
    DebugShow::Instance()->zyrLine(showImage_Result, UpPointNow, DownPointNow,
                                   data_frame_->scalar_color[showColorIndex],
                                   thickness, lineType);

    //        if (SHOW_INDEX)
    //            ShowTextinMat(showImage_Result, Point(UpPointNow.x,
    //            UpPointNow.y), nId);
  }

  //    FinallyResultShow(showImage_Result, g_picture_ID, pBaseDir,
  //    SHOW_WRITER_NINDEX);
  cv::imshow("TestShowLineType", showImage_Result);
}
}
